package novemberBot.navigation;

import lejos.nxt.LCD;
import lejos.util.TimerListener;
import novemberBot.FactoryFloorRobot;
import novemberBot.utils.Debugger;
import novemberBot.utils.LCDInfo;
import novemberBot.utils.Timer2;
import novemberBot.utils.Trig;
/**uses odometer readings and target Position to compute scaling factors for forward and rotation Speed
 * meant to be used with the NavigatorClass only
 * @author Antoine Lennartz*/
public class SpeedController implements TimerListener{
	/**link to odometry*/
	private Odometer odo;
	/**target position*/
	private double[] targPos = new double[3];
	/**unit heading vector*/
	private double[] unitH = new double[2];
	/**target vector*/
	private double[] targV = new double[2];
	/**current position*/
	private double[] cPos = new double[3];
	/**scale for forward Speed*/
	private double forwardScale;
	/**scale for rotation Speed*/
	private double rotationScale;
	/**Lock for arrays*/
	private Object dataLock = new Object();	
	
	private boolean enabled=false;
	
	private boolean allowReverse=false;
	
	private Timer2 t;
	/**constructor*/
	public SpeedController(){
		odo=FactoryFloorRobot.getInstance().getOdometer();
		t = new Timer2(Odometer.DEFAULT_PERIOD, this);
		t.start();
	}
		
	
	@Override
	public void timedOut(){
		if(enabled){
			updateInfo();			
			
		}
	}
	/**used for debugging purposes*/
	@SuppressWarnings("unused")
	private void printStuff(){
		Debugger.pauseLCDI();
		LCD.clear();
		LCD.drawString("dx: "+LCDInfo.truncDouble(targV[0], 3), 0, 0);
		LCD.drawString("dy: "+LCDInfo.truncDouble(targV[1], 3), 0, 1);
		LCD.drawString("fs: "+LCDInfo.truncDouble(forwardScale, 3), 0, 2);
		LCD.drawString("rs: "+LCDInfo.truncDouble(rotationScale, 3), 0, 3);
		if(getDist()<2)LCD.drawString("dA: "+LCDInfo.truncDouble(getdAng(), 3), 0, 4);
		else LCD.drawString("th: "+LCDInfo.truncDouble(Trig.fixAngle2(Trig.atan3(targV[0], targV[1])-cPos[2]), 3), 0, 4);

		LCD.refresh();
	}
	/**gets called by timedOut()
	 * updates the relevant vectors from current odometry position*/
	private void updateInfo(){
		//update cPos
		odo.getPosition(cPos);
		//compute unitH
		synchronized(dataLock){
			unitH[0]=Trig.sin2(cPos[2]);
			unitH[1]=Trig.cos2(cPos[2]);
			//compute targV
			targV[0]=targPos[0]-cPos[0];
			targV[1]=targPos[1]-cPos[1];
		}

	}
	/**computes speed scales when moving to a far awa y point*/
	public void computePointScales(){
		double theta=Trig.fixAngle2(Trig.atan3(targV[0], targV[1])-cPos[2]);
		
		forwardScale=Trig.cos2(theta);
		rotationScale=Trig.sin2(theta);
		if(allowReverse)return;
		if(Math.abs(theta)>20){
			if(rotationScale!=0)rotationScale/=Math.abs(rotationScale);
			else rotationScale=1;
			forwardScale=0;	//if(getDist()>4)
		}else if(Math.abs(forwardScale)<0.2 && getDist()>3)
			forwardScale*=0.2/Math.abs(forwardScale);
			
	
	}
	/**compute speed scales for rotating the robot*/
	public void computeAngleScales(){
		forwardScale=0;
		double theta=getdAng();	
		rotationScale=Trig.sin2(theta);
		
		if(Math.abs(theta)>90)
			rotationScale/=Math.abs(rotationScale);
		else if(Math.abs(rotationScale)<0.4 && Math.abs(theta)>3)
			rotationScale*=0.4/Math.abs(rotationScale);	
		 
	}
	
	
	
	//accessors
	/**accessor for pointScales
	 * @param scales - buffer to write scales into*/
	public void getPointScales(double[] scales){
		
		synchronized(dataLock){
			computePointScales();
			//printStuff();
			//try {
		//		Thread.sleep(20);
		//	} catch (InterruptedException e) {}
			scales[0]=forwardScale;
			scales[1]=rotationScale;
		}

	}
	/**accessor for angleScales
	 * @param scales - buffer to write scales into*/
	public void getAngleScales(double[] scales){
		
		synchronized(dataLock){
			computeAngleScales();
			//printStuff();
		//	try {
		//		Thread.sleep(20);
		//	} catch (InterruptedException e) {}
			scales[0]= forwardScale;
			scales[1]= rotationScale;
	}
		
	}
	
	/**@return distance to target*/
	public double getDist(){
		synchronized(dataLock){
			return Trig.magnitude(targV);
		}
	}
	
	public double getdAng(){
		synchronized(dataLock){
			return Trig.fixAngle2(targPos[2]-cPos[2]);
		}
	}
	
	
	//mutators
	public void setTargPos(double[] targPos){
			this.targPos[0]=targPos[0];
			this.targPos[1]=targPos[1];
			this.targPos[2]=targPos[2];
	}
	/**enables or disables the speedController*/
	public void setEnabled(boolean enabled){
		this.enabled=enabled;
		updateInfo();
	}
	/**sets the boolean which decied weather or not the robot 
	 * is allowed to drive backwards to reach its destination
	 * rather than turn around and goforward
	 * @param allowReverse - new value for boolean*/
	public void setAllowReverse(boolean allowReverse){
		this.allowReverse=allowReverse;
		//updateInfo();
	}
	
	
}
